package scu.maqiang.numeric;

import static java.lang.Math.sqrt;

import java.util.Arrays;

/**
 * 稀疏矩阵迭代求解器
 * @author 马强
 *
 */
public class NewIterSSolver extends Solver{

	/**
	 * 构造函数，传入稀疏矩阵
	 * 
	 * @param mat
	 *            SRMatrix矩阵
	 */
	public NewIterSSolver(SRMatrix mat) {
		int n = mat.getN();
		diag = new double[n];
		matU = new CSRMatrix(n);
		matL = new CSRMatrix(n); 
		CSRMatrix.SRMatrixToCSRMatrix(mat, diag, matU, matL);
	}
	
	public NewIterSSolver(SRMatrix mat, boolean isDelete) {
		this(mat);
		if (isDelete) {
			mat.clear();
		}
	}

	/**
	 * 构造函数，设置求解相关的稀疏矩阵，容许误差以及最大迭代次数
	 * 
	 * @param mat
	 *            SRMatrix矩阵
	 * @param eps
	 *            容许误差
	 * @param maxIteration
	 *            最大迭代次数
	 */
	public NewIterSSolver(SRMatrix mat, double eps, int maxIteration) {
		this(mat);
		this.eps = eps;
		this.maxIteration = maxIteration;
	}

	
	public NewIterSSolver() {
	}

	public void setMatrix(SRMatrix mat) {
		int n = mat.getN();
		diag = new double[n];
		matU = new CSRMatrix(n);
		matL = new CSRMatrix(n);
		CSRMatrix.SRMatrixToCSRMatrix(mat, diag, matU, matL);
	}

	/**
	 * 共轭梯度法求解对称正定线性方程组(已达最优)
	 * 
	 * @param b
	 *            右端项
	 * @param x0
	 *            迭代初始值, 同时保存迭代解
	 * @param verbosity
	 *            冗余参数，
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public void CG(double[] b, double[] x0, int verbosity) {
		long begin = System.nanoTime();
		int n = matU.getM();
		double[] Ap = matU.mul(x0);
		double[] r = MVO.add(b, -1.0, Ap);
		double[] p = (double[]) r.clone();
		double r2 = MVO.dot_product(r, r);
		double temp1, alpha, beta;
		int i, k;
		double[] x = x0.clone();
		Arrays.setAll(x, idx -> x0[idx] + 1);
		double[] diff = null;
		for (i = 0; i < maxIteration; i++) {
			//System.out.println(MVO.L2Norm(r0));
			diff = MVO.add(x0, -1.0, x);
			if (Math.max(MVO.L2Norm(diff), sqrt(r2)) < eps) {
				break;
			}
			Arrays.setAll(x, idx -> x0[idx]);
			matU.mul(p, Ap);
			temp1 = MVO.dot_product(Ap, p);
			alpha = r2 / temp1;
			temp1 = 0;
			for (k = 0; k < n; k++) {
				x0[k] += alpha * p[k];
				r[k] -= alpha * Ap[k];
				temp1 += r[k] * r[k];
			}
			beta = temp1 / r2;
			r2 = temp1;
			for (k = 0; k < n; k++) {
				p[k] = r[k] + beta * p[k];
			}
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (i + 1));
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10f\n", sqrt(r2));
				System.out.println();
			}
			//System.out.println(Arrays.toString(x0));
			//System.out.println(Arrays.toString(x));
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
	}

	/**
	 * 共轭梯度法方程误差(CGNE)法求解一般线性方程组(已达最优)
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @param verbosity 输出控制参数
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int CGNE(double[] b, double[] x0, int verbosity) {
		int n = b.length;
		long begin = System.nanoTime();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] p0 = matU.mulT(r0);
		double[] Ap0 = new double[n];
		double temp1, temp2, alpha, beta, r2;
		r2 = MVO.L2Norm(r0);
		int i;
		for (i = 0; i < maxIteration; i++) {
			if (r2 < eps) {
				break;
			}
			temp1 = 0.0;
			temp2 = 0.0;
			matU.mul(p0, Ap0);
			for (int j = 0; j < n; j++) {
				temp1 += r0[j] * r0[j];
				temp2 += p0[j] * p0[j];
			}
			alpha = temp1 / temp2 ; 
			temp2 = temp1;
			temp1 = 0.0;
			for (int j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * Ap0[j];
				temp1 += r0[j] * r0[j];
			}
			beta = temp1 / temp2;
			matU.mulT(r0, Ap0);
			for (int j = 0; j < n; j++) {
				p0[j] = Ap0[j] + beta * p0[j];
			}
			r2 = MVO.L2Norm(r0);
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (i + 1));
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10f\n", r2);
				System.out.println();
			}
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			throw new IllegalArgumentException("Iteration Has Reached Maximum Time");
		}
		return i;
	}
	
	public int PCGNE_M(double[] b, double[] x0, int verbosity) {
		int n = b.length;
		long begin = System.nanoTime();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] z0 = new double[n];
		//CSRMatrix.SSORTreat(diag, matU, matL, r0, z0, 1.5);
		for(int i = 0; i < n; i++) {
			z0[i] = r0[i] / diag[i];
		}
		CSRMatrix.GSPrecondition(diag, matU, r0, z0);
		double[] p0 = matU.mulT(z0);
		double[] Ap0 = new double[n];
		double temp1, temp2, alpha, beta, r2;
		r2 = MVO.L2Norm(r0);
		int i;
		for (i = 0; i < maxIteration; i++) {
			if (r2 < eps) {
				break;
			}
			temp1 = 0.0;
			temp2 = 0.0;
			matU.mul(p0, Ap0);
			for (int j = 0; j < n; j++) {
				temp1 += z0[j] * r0[j];
				temp2 += p0[j] * p0[j];
			}
			alpha = temp1 / temp2 ; 
			temp2 = temp1;
			for (int j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * Ap0[j];
				z0[j] = r0[j] / diag[j];
			}
			//for(int k = 0; k < n; k++) {
			//	
			//}
			//CSRMatrix.SSORTreat(diag, matU, matL, r0, z0, 1.5);
			//CSRMatrix.GSPrecondition(diag, matU, r0, z0);
			temp1 = MVO.dot_product(z0, r0);
			beta = temp1 / temp2;
			matU.mulT(z0, Ap0);
			for (int j = 0; j < n; j++) {
				p0[j] = Ap0[j] + beta * p0[j];
			}
			r2 = MVO.L2Norm(r0);
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (i + 1));
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10f\n", r2);
				System.out.println();
			}
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			throw new IllegalArgumentException("Iteration Has Reached Maximum Time");
		}
		return i;
	}
	/**
	 * 对称逐次超松弛预处理共轭梯度法求解对称正定线性方程组(已达最优)
	 * 
	 * @param b
	 *            右端项
	 * @param x0
	 *            迭代初始值, 同时保存迭代解
	 * @param omega
	 *            松弛因子
	 * @param verbosity
	 *            冗余参数，
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int PCGSSOR(double[] b, double[] x0, double omega, int verbosity) {
		int n = matU.getN();
		long begin = System.nanoTime();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		if (MVO.L2Norm(r0) < eps) {
			return 0;
		}
		double[] z0 = new double[n];
		CSRMatrix.SSORTreat(diag, matU, matL, r0, z0, omega);

		double[] z1 = z0.clone();
		double[] p0 = z0.clone();

		int i, j;
		double temp1, temp2, alpha, beta;
		double[] x = x0.clone();
		Arrays.setAll(x, idx-> x0[idx] + 1);
		for (i = 0; i < maxIteration; i++) {
			//System.out.println(MVO.L2Norm(r0));
			double[] diff = MVO.add(x0, -1.0, x);
			if (Math.max(MVO.L2Norm(diff), MVO.L2Norm(r0)) < eps) {
				break;
			}
			Arrays.setAll(x, idx -> x0[idx]);
			temp1 = temp2 = 0.0;
			matU.mul(p0, z1);
			for (j = 0; j < n; j++) {
				temp1 += z0[j] * r0[j];
				temp2 += z1[j] * p0[j];
			}
			alpha = temp1 / temp2;
			for (j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * z1[j];
			}

			// System.out.println("r0 = " + MVO.toString(r0));
			// SSORTreat(A, diag, r0, z1, omega);
			CSRMatrix.SSORTreat(diag, matU, matL, r0, z1, omega);

			temp2 = temp1;
			temp1 = 0.0;
			for (j = 0; j < n; j++) {
				temp1 += z1[j] * r0[j];
			}
			beta = temp1 / temp2;
			for (j = 0; j < n; j++) {
				p0[j] = z1[j] + beta * p0[j];
				z0[j] = z1[j];
			}
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (i + 1));
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10f\n", MVO.L2Norm(r0));
				System.out.println();
			}
			// System.arraycopy(r1, 0, r0, 0, n);
			// System.arraycopy(z1, 0, z0, 0, n);
		}

		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			return -1;
		}
		r0 = null;
		z0 = null;
		z1 = null;
		p0 = null;
		return i;
	}

	/**
	 * 
	 * @param b
	 * @param x0
	 * @param omega
	 * @param verbosity
	 * @return
	 */
	public int PCGSSOR(double[][] b, double[][] x0, double omega, int verbosity) {
		int len = b.length;
		int n = matU.getN();
		long begin = System.nanoTime();
		boolean[] converged = new boolean[len];
		int allconverged = 0;
		double[][] r0 = new double[len][];
		double[][] z0 = new double[len][n];
		double[][] z1 = new double[len][];
		double[][] p0 = new double[len][];
		for (int i = 0; i < len; i++) {
			r0[i] = MVO.add(b[i], -1.0, matU.mul(x0[i]));
		}

		for (int i = 0; i < len; i++) {
			CSRMatrix.SSORTreat(diag, matU, matL, r0[i], z0[i], omega);
		}

		for (int i = 0; i < len; i++) {
			z1[i] = z0[i].clone();
			p0[i] = z0[i].clone();
		}

		int i, j, k;
		double temp1, temp2, alpha, beta;
		for (k = 0; k < maxIteration; k++) {
			for (i = 0; i < len; i++) {
				if (MVO.L2Norm(r0[i]) < eps) {
					converged[i] = true;
					allconverged++;
				}
			}
			if (allconverged == len) {
				break;
			}
			for (i = 0; i < len && !converged[i]; i++) {
				temp1 = temp2 = 0.0;
				matU.mul(p0[i], z1[i]);
				for (j = 0; j < n; j++) {
					temp1 += z0[i][j] * r0[i][j];
					temp2 += z1[i][j] * p0[i][j];
				}
				alpha = temp1 / temp2;
				for (j = 0; j < n; j++) {
					x0[i][j] += alpha * p0[i][j];
					r0[i][j] -= alpha * z1[i][j];
				}

				CSRMatrix.SSORTreat(diag, matU, matL, r0[i], z1[i], omega);

				temp2 = temp1;
				temp1 = 0.0;
				for (j = 0; j < n; j++) {
					temp1 += z1[i][j] * r0[i][j];
				}
				beta = temp1 / temp2;
				for (j = 0; j < n; j++) {
					p0[i][j] = z1[i][j] + beta * p0[i][j];
					z0[i][j] = z1[i][j];
				}

			}
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (k + 1));
				for (i = 0; i < len; i++) {
					System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0[i]), MVO.max(x0[i]));
					System.out.printf("Error: %20.10f\n", MVO.L2Norm(r0[i]));
				}
				System.out.println();
			}
		}

		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + k);
			for (i = 0; i < len; i++) {
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0[i]), MVO.max(x0[i]));
			}
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (k == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			return -1;
		}
		return k;
	}

	
	/**
	 * 对称逐次超松弛预处理共轭梯度平方法(CGS)求解一般线性方程组
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int PCGSSSOR(double[] b, double[] x0, double omega, int verbosity) {
		int n = matU.getN();
		long begin = System.nanoTime();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] z0 = new double[n];
		CSRMatrix.SSORTreat(diag, matU, matL, r0, z0, omega);
		double[] u0 = z0.clone();
		double[] p0 = z0.clone();
		double[] q0 = new double[n];
		double[] Ap0 = matU.mul(p0);
		double[] v0 = new double[n];
		double[] rr0 = MVO.add(z0, 1.0);
		
		CSRMatrix.SSORTreat(diag, matU, matL, Ap0, v0, omega);
		int k;
		for (k = 0; k < maxIteration; k++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
//			if (k <= 3) {
//				System.out.println("Error = " + MVO.L2Norm(r0));
//			}
			//System.out.println(MVO.L2Norm(r0) + "OK");
			double temp1 = 0.0;
			double temp2 = 0.0;
			for(int j = 0; j < n; j++) {
				temp1 += z0[j] * rr0[j];
				temp2 += v0[j] * rr0[j];
			}
			double alpha = temp1 / temp2;
			
			for(int i = 0; i < n; i++) {
				q0[i] = u0[i] - alpha * v0[i];
				v0[i] = u0[i] + q0[i];
			}
			
			matU.mul(v0, u0);
			for(int i = 0; i < n; i++) {
				x0[i] += alpha * v0[i];
				r0[i] -= alpha * u0[i];
			}
			CSRMatrix.SSORTreat(diag, matU, matL, r0, z0, omega);
			
			temp2 = temp1;
			temp1 = 0.0;
			for(int i = 0; i < n; i++) {
				temp1 += z0[i] * rr0[i];
			}
			double beta = temp1 / temp2;
			for(int i = 0; i < n; i++) {
				u0[i] = z0[i] + beta * q0[i];
				p0[i] = u0[i] + beta *(q0[i] + beta * p0[i]);
			}
			
			matU.mul(p0, Ap0);
			CSRMatrix.SSORTreat(diag, matU, matL, Ap0, v0, omega);
			if (verbosity >= 2) {
				System.out.println();
				System.out.println("Iteration: " + (k + 1));
				System.out.printf("min: %20.10e\t max: %20.10e\n", MVO.min(x0), MVO.max(x0));
				System.out.printf("Error: %20.10e\n", MVO.L2Norm(r0));
				System.out.println();
			}
		}
		
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + k);
			System.out.printf("min: %20.10e\t max: %20.10e\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		
		if(k == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			return -1;
		}
		//System.out.println("Iteration Time " + k);
		return k;
	}
	
	/**
	 * 
	 * @param b
	 * @param x
	 * @param m
	 */
	public void GMRES(double[] b, double[] x, int m) {
		int n = matU.getM();
		ParamCheck.checkEqual(n, b.length);
		ParamCheck.checkEqual(n, x.length);
		double[][] H = new double[m + 1][m];
		double[][] Q = new double[n][m + 1];
		double[] x0 = x.clone();
		double[] f = new double[m + 1];
		boolean flag = true;
		int iter = 0;
		while (flag && iter < maxIteration) {
			double[] r = MVO.add(b, -1.0, matU.mul(x));
			f[0] = MVO.L2Norm(r);
			for (int i = 0; i < n; i++) {
				Q[i][0] = r[i] / f[0];
			}
			for (int k = 0; k < m; k++) {
				double[] colK = MVO.col(Q, k);
				double[] y = matU.mul(colK);
				for (int j = 0; j <= k; j++) {
					double[] colJ = MVO.col(Q, j);
					H[j][k] = MVO.dot_product(colJ, y);
					for (int t = 0; t < n; t++) {
						y[t] -= H[j][k] * colJ[t];
					}
				}
				H[k + 1][k] = MVO.L2Norm(y);
				for (int t = 0; t < n; t++) {
					Q[t][k + 1] = y[t] / H[k + 1][k];
				}
				double[] c = new double[k + 1];
				double error = IterDSolver.MLS_QR_Givens(MVO.subMatrix(H, 0, k + 1, 0, k), MVO.subVector(f, 0, k + 1), c);
				MVO.add(x0, 1.0, MVO.matmul(MVO.subMatrix(Q, 0, n - 1, 0, k), c), x);
				System.out.println("Error = " + error + "    Iter = " + iter);
				if (Math.abs(error) < Constants.Er) {
					flag = false;
					break;
				}
				iter++;
			}
			for(int i = 0; i < n; i++) {
				x0[i] = x[i];
			}
		}
	}

	/**
	 * 
	 * @param b
	 * @param x
	 * @param omega
	 * @param m
	 * @param verbosity
	 */
	public void GMRES_SSOR(double[] b, double[] x, double omega, int m, int verbosity) {
		int n = matU.getM();
		ParamCheck.checkEqual(n, b.length);
		ParamCheck.checkEqual(n, x.length);
		long begin = System.nanoTime();
		double[][] H = new double[m + 1][m];
		double[][] Q = new double[n][m + 1];
		double[] x0 = x.clone();
		double[] f = new double[m + 1];
		double[] z0 = new double[n];
		double[] r = new double[n];
		boolean flag = true;
		int iter = 0;
		while (flag && iter < maxIteration) {
			MVO.add(b, -1.0, matU.mul(x), r);
			CSRMatrix.SSORTreat(diag, matU, matL, r, z0, omega);
			r = z0.clone();
			f[0] = MVO.L2Norm(r);
			for (int i = 0; i < n; i++) {
				Q[i][0] = r[i] / f[0];
			}
			for (int k = 0; k < m; k++) {
				double[] colK = MVO.col(Q, k);
				CSRMatrix.SSORTreat(diag, matU, matL, matU.mul(colK), z0, omega);
				double[] y = z0.clone();
				for (int j = 0; j <= k; j++) {
					double[] colJ = MVO.col(Q, j);
					H[j][k] = MVO.dot_product(colJ, y);
					for (int t = 0; t < n; t++) {
						y[t] -= H[j][k] * colJ[t];
					}
				}

				H[k + 1][k] = MVO.L2Norm(y);
				for (int t = 0; t < n; t++) {
					Q[t][k + 1] = y[t] / H[k + 1][k];
				}
				//System.out.println(MVO.toString(H));
				double[] c = new double[k + 1];
				double error = IterDSolver.MLS_QR_Givens(MVO.subMatrix(H, 0, k + 1, 0, k), MVO.subVector(f, 0, k + 1), c);
				//double error = IterDSolver.MLS_QR_Givens2(H, 0, k + 1, 0, k, f, c);
				//System.out.println(Arrays.toString(c));
				MVO.add(x0, 1.0, MVO.matmul(MVO.subMatrix(Q, 0, n - 1, 0, k), c), x);
				if (verbosity >= 2) {
					System.out.println("Error = " + error + "    iter = " + iter);
					System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x), MVO.max(x));
//					if (k == 3) {
//						System.exit(0);
//					}
				}
				if (Math.abs(error) < eps) {
					flag = false;
					break;
				}
				iter++;
			}
			for(int i = 0; i < n; i++) {
				x0[i] = x[i];
			}
		}
		
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + iter);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x), MVO.max(x));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
	}
	
	/**
	 * 共轭梯度法方程残量(CGNR)法求解一般线性方程组(已达最优)
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int CGNR(double[] b, double[] x0, int verbosity) {
		int n = b.length;
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] p0 = matU.mulT(r0);
		double[] Ap0 = new double[n];
		int i;
		long begin = System.nanoTime();
		for (i = 0; i < maxIteration; i++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
			double temp1 = 0.0;
			double temp2 = 0.0;
			Arrays.fill(Ap0, 0.0);
			matU.mulT(r0, Ap0);
			temp1 = MVO.dot_product(Ap0, Ap0);
			matU.mul(p0, Ap0);
			temp2 = MVO.dot_product(Ap0, Ap0);
			double alpha = temp1 / temp2;
			for (int j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * Ap0[j];
			}
			
			temp2 = temp1;
			temp1 = 0.0;
			//Arrays.fill(Ap0, 0.0);
			matU.mulT(r0, Ap0);
			temp1 = MVO.dot_product(Ap0, Ap0);
			double beta = temp1 / temp2;
			for (int j = 0; j < n; j++) {
				p0[j] = Ap0[j] + beta * p0[j];
			}

			if (verbosity >= 2) {
				System.out.println("Error = " + MVO.L2Norm(r0) + "    iter = " + i);
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
//				if (k == 3) {
//					System.exit(0);
//				}
			}
//			System.out.println("Iteration " + (i + 1) + ": min\t"
//					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time");
			return -1;
		}
		//System.out.println("Iteration Time " + i);
		return i;
	}
	
	public int PCGNR_SSOR(double[] b, double[] x0, int verbosity) {
		int n = b.length;
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] p0 = matU.mulT(r0);
		double[] Ap0 = new double[n];
		int i;
		long begin = System.nanoTime();
		for (i = 0; i < maxIteration; i++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
			double temp1 = 0.0;
			double temp2 = 0.0;
			Arrays.fill(Ap0, 0.0);
			matU.mulT(r0, Ap0);
			temp1 = MVO.dot_product(Ap0, Ap0);
			matU.mul(p0, Ap0);
			temp2 = MVO.dot_product(Ap0, Ap0);
			double alpha = temp1 / temp2;
			for (int j = 0; j < n; j++) {
				x0[j] += alpha * p0[j];
				r0[j] -= alpha * Ap0[j];
			}
			
			temp2 = temp1;
			temp1 = 0.0;
			//Arrays.fill(Ap0, 0.0);
			matU.mulT(r0, Ap0);
			temp1 = MVO.dot_product(Ap0, Ap0);
			double beta = temp1 / temp2;
			for (int j = 0; j < n; j++) {
				p0[j] = Ap0[j] + beta * p0[j];
			}

			if (verbosity >= 2) {
				System.out.println("Error = " + r0 + "    iter = " + i);
				System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
//				if (k == 3) {
//					System.exit(0);
//				}
			}
//			System.out.println("Iteration " + (i + 1) + ": min\t"
//					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		if (verbosity >= 1) {
			System.out.println();
			System.out.println("Total iteration: " + i);
			System.out.printf("min: %20.10f\t max: %20.10f\n", MVO.min(x0), MVO.max(x0));
			long end = System.nanoTime();
			System.out.println("Solve time: " + (end - begin) / 1.0e9 + " s");
			System.out.println();
		}
		if (i == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time");
			return -1;
		}
		//System.out.println("Iteration Time " + i);
		return i;
	}
	
	
	/**
	 * 共轭梯度平方法(CGS)求解一般线性方程组
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int CGS(double[] b, double[] x0) {
		int n = matU.getN();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] u0 = r0.clone();
		double[] z0 = MVO.add(r0, 1.0);
		double[] p0 = r0.clone();
		double[] q0 = new double[n];
		double[] v0 = matU.mul(p0);
		//double alpha = 0.0;
		//double beta = 0.0;
		int k;
		for (k = 0; k < maxIteration; k++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
			
			double temp1 = 0.0;
			double temp2 = 0.0;
			for(int j = 0; j < n; j++) {
				temp1 += r0[j] * z0[j];
				temp2 += v0[j] * z0[j];
			}
			double alpha = temp1 / temp2;
//			for(int i = 0; i < n; i++) {
//				q0[i] = u0[i] - alpha * v0[i];
//			}
//			double[] uq = MVO.add(u0, q0);
//			double[] Auq = A.mul(uq);
//			
//			temp2 = temp1;
//			temp1 = 0.0;
//			for(int i = 0; i < n; i++) {
//				x0[i] += alpha * uq[i];
//				r0[i] -= alpha * Auq[i];
//				temp1 += r0[i] * z0[i];
//			}
			for(int i = 0; i < n; i++) {
				q0[i] = u0[i] - alpha * v0[i];
				v0[i] = u0[i] + q0[i];

			}
			
//			System.out.println("v0 = " + MVO.toString(v0));
			//double[] uq = MVO.add(u0, q0);
			//double[] Auq = A.mul(v0);
			temp2 = temp1;
			temp1 = 0.0;
			matU.mul(v0, u0);
			for(int i = 0; i < n; i++) {
				x0[i] += alpha * v0[i];
				r0[i] -= alpha * u0[i];
				temp1 += r0[i] * z0[i];
			}
			double beta = temp1 / temp2;
			for(int i = 0; i < n; i++) {
				u0[i] = r0[i] + beta * q0[i];
				p0[i] = u0[i] + beta *(q0[i] + beta * p0[i]);

			}
			matU.mul(p0, v0);
			//v0 = A.mul(p0);
			System.out.println("Iteration " + (k + 1) + ": min\t"
					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		if(k == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time! ");
			return -1;
		}
		System.out.println("Iteration Time " + k);
		return k;
	}
	
	/**
	 * 双共轭梯度法(BiCG)求解一般线性方程组
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int BiCG(double[] b, double[] x0) {
		int n = matU.getN();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] r1 = r0.clone();
		double[] p0 = r0.clone();
		double[] z0 = r0.clone();
		double[] z1 = new double[n];
		double[] pt = z0.clone();
		int i;
		for(i = 0; i < maxIteration; i++) {
			if (MVO.L2Norm(r0) < eps) {
				break;
			}
			double[] Ap0 = matU.mul(p0);
			double alpha = MVO.dot_product(r0, z0)/MVO.dot_product(Ap0, pt);
			double[] Atpt = matU.mulT(pt);
			for(int j = 0; j < n; j++) {
				x0[j] = x0[j] + alpha * p0[j];
				r1[j] = r0[j] - alpha * Ap0[j];
				z1[j] = z0[j] - alpha * Atpt[j];
			}
			System.out.println("z1 = " + MVO.toString(z1));
			double beta = MVO.dot_product(r1, z1)/MVO.dot_product(r0, z0);
			System.out.println(beta);
			for(int j = 0; j < n; j++) {
				p0[j] = r1[j] + beta * p0[j];
				pt[j] = z1[j] + beta * pt[j];
				z0[j] = z1[j];
				r0[j] = r1[j];
			}
			System.out.println("Iteration " + (i + 1) + ": min\t"
					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		if (i == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time");
			return -1;
		}
		System.out.println("Iteration Time " + i);
		return i;
	}
	
	/**
	 * 双共轭梯度稳定化方法求解线性方程组
	 * @param A 一般非奇异稀疏矩阵
	 * @param b 右端项
	 * @param x0 迭代初始值，并返回迭代解
	 * @return 迭代次数, 若迭代超过最大迭代次数，返回-1
	 */
	public int BiCGSTAB(double[] b, double[] x0) {
		int n = matU.getN();
		double[] r0 = MVO.add(b, -1.0, matU.mul(x0));
		double[] p0 = r0.clone();
		double[] z0 = r0.clone();
		int i;
		for(i = 0; i < maxIteration; i++) {
			if(MVO.L2Norm(r0) < Constants.Er) {
				break;
			}
			System.out.println("Error = " + MVO.L2Norm(r0));
			double[] Ap0 = matU.mul(p0);
			double temp = MVO.dot_product(r0, z0);
			double alpha = temp/ MVO.dot_product(Ap0, z0);
			for(int j = 0; j < n; j++) {
				x0[j] = x0[j] + alpha * p0[j];
				r0[j] = r0[j] - alpha * Ap0[j];
			}
			double[] Ar0 = matU.mul(r0);
			double omega = MVO.dot_product(r0, Ar0)/MVO.dot_product(Ar0, Ar0);
			for(int j = 0; j < n; j++) {
				x0[j] = x0[j] + omega * r0[j];
				r0[j] = r0[j] - omega * Ar0[j];
			}
			double beta = alpha /omega * MVO.dot_product(r0, z0) / temp;
			for(int j = 0; j < n; j++) {
				p0[j] = r0[j] + beta *(p0[j] - omega * Ap0[j]);
			}
//			System.out.println("Iteration " + (i + 1) + ": min\t"
//					+ MVO.min(x0) + "\tmax\t" + MVO.max(x0));
		}
		
		if (i == maxIteration) {
			System.out.println("Iteration Has Reached Maximum Time");
			return -1;
		}
		System.out.println("Iteration Time " + i);
		return i;
	}

	private CSRMatrix matU;
	private CSRMatrix matL;
	private double[] diag;
}
